package com.gitee.wsl.mathematics.vector.ext

import com.gitee.wsl.mathematics.interpolator.ext.FULL
import com.gitee.wsl.mathematics.number.compareTo
import com.gitee.wsl.mathematics.vector.vec2.MutableVector2
import com.gitee.wsl.mathematics.vector.vec2.Vec2
import com.gitee.wsl.mathematics.vector.vec2.Vec2f
import com.gitee.wsl.mathematics.vector.vec2.Vector2
import com.gitee.wsl.mathematics.vector.vec3.Vector3
import com.gitee.wsl.unit.angle.AngleUnit
import com.gitee.wsl.unit.angle.ext.cos
import com.gitee.wsl.unit.angle.ext.cosF
import com.gitee.wsl.unit.angle.ext.plus
import com.gitee.wsl.unit.angle.ext.sin
import com.gitee.wsl.unit.angle.ext.sinF
import com.gitee.wsl.unit.angle.ext.tan
import com.gitee.wsl.unit.angle.ext.unaryMinus
import com.gitee.wsl.unit.angle.toRadians

val <T:Number> Vector2<T, *>.angle get() = kotlin.math.atan2(y.toDouble(),x.toDouble()).toRadians()

/**
 * Returns the angle in radians between the line connecting the two points and the x coordinate.
 */
//fun <T:Number> Vector2<T, *>.angleToPoint(other: Vector2<T, *>) = (other - this).angle


fun Vec2(angleUnit: AngleUnit) = Vec2(angleUnit.cos(),angleUnit.sin())

fun Vec2f(angleUnit: AngleUnit) = Vec2f(angleUnit.cosF(),angleUnit.sinF())

fun rotateX(x: Double, y: Double, angleRad: AngleUnit): Double {
    return x * angleRad.cos() - y * angleRad.sin()
}

fun rotateY(x: Double, y: Double, angleRad: AngleUnit): Double {
    return x * angleRad.sin() + y * angleRad.cos()
}

fun rotateCenteredX(x: Double, y: Double, centerX: Double, centerY: Double, angleRad: AngleUnit): Double {
    return centerX + (x - centerX) * angleRad.cos() - (y - centerY) * angleRad.sin()
}

fun rotateCenteredY(x: Double, y: Double, centerX: Double, centerY: Double, angleRad: AngleUnit): Double {
    return centerY + (x - centerX) * angleRad.sin() + (y - centerY) * angleRad.cos()
}

fun<T:Number> Vector2<T, *>.rotate(angleRad: AngleUnit) = create(rotateX(x.toDouble(),y.toDouble(),angleRad)+zero, rotateY(x.toDouble(),y.toDouble(),angleRad)+zero)

fun<T:Number> Vector2<T, *>.rotate(center:Vector2<T,*>,angleRad: AngleUnit) = create(
    rotateCenteredX(x.toDouble(),y.toDouble(),center.x.toDouble(),center.y.toDouble(),angleRad)+zero,
    rotateCenteredY(x.toDouble(),y.toDouble(),center.x.toDouble(),center.y.toDouble(),angleRad)+zero
)

fun<T:Number> MutableVector2<T, *>.rotateBy(angleRad: AngleUnit) = set(rotateX(x.toDouble(),y.toDouble(),angleRad)+zero, rotateY(x.toDouble(),y.toDouble(),angleRad)+zero)

fun<T:Number> MutableVector2<T, *>.rotateBy(center:Vector2<T,*>,angleRad: AngleUnit) = set(
    rotateCenteredX(x.toDouble(),y.toDouble(),center.x.toDouble(),center.y.toDouble(),angleRad)+zero,
    rotateCenteredY(x.toDouble(),y.toDouble(),center.x.toDouble(),center.y.toDouble(),angleRad)+zero
)

fun AngleUnit.adjustFromUp(up: Vector2<*, *>): AngleUnit {
    //Orientation.checkValidUpVector(up)
    return if (up.y > 0) this else -this
}

fun AngleUnit.cosine(up: Vector2<*, *> = Vec2.up): Double = adjustFromUp(up).cos()
fun AngleUnit.sine(up: Vector2<*, *> = Vec2.up): Double = adjustFromUp(up).sin()
fun AngleUnit.tangent(up: Vector2<*, *> =Vec2.up): Double = adjustFromUp(up).tan()

fun Angle_between(x0: Double, y0: Double, x1: Double, y1: Double, up: Vector2<*, *> = Vec2.up): AngleUnit {
    val angle = AngleUnit.atan2(y1 - y0, x1 - x0)
    return (if (angle < AngleUnit.ZERO) angle + AngleUnit.FULL else angle).adjustFromUp(up)
}

inline fun AngleUnit.Companion.atan2(x: Double, y: Double, up: Vector2<*, *> = Vec2.up): AngleUnit = fromRadians(kotlin.math.atan2(x, y)).adjustFromUp(up)
inline fun AngleUnit.Companion.atan2(p: Vector2<*, *>, up: Vector2<*, *> = Vec2.up): AngleUnit = atan2(p.x.toDouble(), p.y.toDouble(), up)


inline fun AngleUnit.Companion.between(x0: Double, y0: Double, x1: Double, y1: Double, up: Vector2<*, *> = Vec2.up) = Angle_between(x0, y0, x1, y1, up)
inline fun AngleUnit.Companion.between(x0: Float, y0: Float, x1: Float, y1: Float, up: Vector2<*, *> = Vec2.up)= between(x0.toDouble(), y0.toDouble(), x1.toDouble(), y1.toDouble(), up)
inline fun AngleUnit.Companion.between(p0: Vector2<*, *> , p1: Vector2<*, *> , up:Vector2<*, *> = Vec2.up) = between(p0.x.toDouble(), p0.y.toDouble(), p1.x.toDouble(), p1.y.toDouble(), up)

inline fun AngleUnit.Companion.between(ox: Double, oy: Double, x1: Double, y1: Double, x2: Double, y2: Double, up: Vector2<*, *> = Vec2.up)= between(x1 - ox, y1 - oy, x2 - ox, y2 - oy, up)
inline fun AngleUnit.Companion.between(ox: Float, oy: Float, x1: Float, y1: Float, x2: Float, y2: Float, up: Vector2<*, *> = Vec2.up)= between(x1 - ox, y1 - oy, x2 - ox, y2 - oy, up)
inline fun AngleUnit.Companion.between(o: Vector2<*, *> , v1: Vector2<*, *> , v2: Vector2<*, *> , up: Vector2<*, *> = Vec2.up) = between(o.x.toDouble(), o.y.toDouble(), v1.x.toDouble(), v1.y.toDouble(), v2.x.toDouble(), v2.y.toDouble(), up)

/**
 * Returns the angle in radians between this vector and the vector
 * parameter; the return value is constrained to the range [0,PI].
 * @return   the angle in radians in the range [0,PI]
 */
fun<T:Number> Vector3<T, *>.angle(other: Vector3<T, *>):AngleUnit  {
    var vDot = this.dot(other) / (this.length * other.length)
    if (vDot < -1f) vDot = -1f + zero
    if (vDot > 1f) vDot = one
    return acos(vDot).toRadians()
}

fun<T:Number> Vector3<T, *>.angleXZ(other: Vector3<T, *>) = atan2(x * other.z - z * other.x, x * other.x + z * other.z).toRadians()

fun<T:Number> Vector3<T, *>.angleXY(other: Vector3<T, *>) = atan2(x * other.y - y * other.x, x * other.x + y * other.y).toRadians()